% calculate the outputs for AMBER model
% outputs include: 1. hip position
%                  2. linearized hip position
%                  3. non-stance slope
%                  4. linearized non-stance slope
%                  5. stance torso slope
%                  6. linearized stance torso slope
%                  7. non-stance torso slope
%                  8. linearized non-stance torso slope
%                  9. COM
%                  10 linearized COM
function outputs = cal_robot_para(q,model_name)
if strcmp(model_name,'AMBER')
addpath('models/AMBER/build_torso');
elseif strcmp(model_name,'NAO')
addpath('models/NAO/build_torso');
end
for i=1:size(q,2)
    % 1. hip position
    hippos(i) = hipposFn(q(:,i));
    % 2. linearized hip position
    delta_hippos(i) = deltaHipposFn(q(:,i));
    % 3. nonstance slope
    nsslope(i) = nsslopeFn(q(:,i));
    % 4. linearized non-stance slope
    delta_nsslope(i) = deltaNSslopeFn(q(:,i));
    % torso related angle
    % 5. stance torso
    storso(i) = storsoFn(q(:,i));
    % 6. linearized stantce torso slope
    delta_storso(i) = deltaStorsoFn(q(:,i));
    % 7. non-stance torso
    nstorso(i) = nstorsoFn(q(:,i));
    % 8. linearized non-stance torso
    delta_nstorso(i) = deltaNStorsoFn(q(:,i));
    % 9. COM
    com(i) = comFn(q(:,i));
    % 10. linearized COM
    delta_com(i) = deltaCOMFn(q(:,i));
end
outputs.hippos = hippos-hippos(1);
outputs.delta_hippos = delta_hippos-delta_hippos(1);
outputs.nsslope = nsslope;
outputs.delta_nsslope = delta_nsslope;
outputs.storso = storso;
outputs.delta_storso = delta_storso;
outputs.nstorso = nstorso;
outputs.delta_nstorso = delta_nstorso;
outputs.com = com;
outputs.delta_com = delta_com;
end